;Edit the raw code into chain length reader Jia20160301
;raw code:GPT10_combined_image_position_ver2_Jia20150313 
;
;Jia wrote on 20150313
;The code is wrote to combine the track of two camera to construct the path of the chain
;for the ruler, 0 is at the center of the camrea with right +/left -
;so --:forward_back/left_back   ->0~pi/2      |     |            -
;   -+:forward_back/left_front  ->pi/2~pi     | L + |  -   -------------- 
;   ++:forward_front/left_front ->pi~3pi/2    |     |            +
;   +-:forward_front/left_back  ->3pi/2 2pi   |     |            F
;
;raw_data_path='D:\working space\GPT10\*.txt'
;raw data from                
;index     F_frame   F_X(head)   F_X(tail)  F_X1(bead)  F_Y1(bead)  F_X8(bead)  F_Y8(bead)  F_threshold    L_frame   L_X(head)   L_X(tail)  L_X1(bead)  L_Y1(bead)  
;F_L8(bead)  F_L8(bead)  L_threshold       F_Xc        L_Xc      F_side      L_side      F_mode      L_mode  decide(1F2L)  Mode  
;
;output data form
;index F_X1(head) F_X2(tail) L_X1(head) L_X2(tail) F_X1(after scaling) F_X2(afterscaling) L_X1(after scaling) L_X2(afterscaling)

function GPT10_chain_length_reader,raw_data_path,translation
  ;ruler form: pixel position angle
  forward_front_data_path='D:\working space\GPT10\GPT10_background_rescaling_for_GPT10_20150212N=5\rescaling\GPT10_20160723_image_scaling_F_camera_front_ruler.txt'
  forward_back_data_path='D:\working space\GPT10\GPT10_background_rescaling_for_GPT10_20150212N=5\rescaling\GPT10_20160723_image_scaling_F_camera_back_ruler.txt'
  left_front_data_path='D:\working space\GPT10\GPT10_background_rescaling_for_GPT10_20150212N=5\rescaling\GPT10_20160723_image_scaling_L_camera_front_ruler.txt'
  left_back_data_path='D:\working space\GPT10\GPT10_background_rescaling_for_GPT10_20150212N=5\rescaling\GPT10_20160723_image_scaling_L_camera_back_ruler.txt'

  forward_front_data=read_ascii(forward_front_data_path,data_start=1)
  forward_front_data=forward_front_data.(0)
  forward_back_data=read_ascii(forward_back_data_path,data_start=1)
  forward_back_data=forward_back_data.(0)
  left_front_data=read_ascii(left_front_data_path,data_start=1)
  left_front_data=left_front_data.(0) 
  left_back_data=read_ascii(left_back_data_path,data_start=1)
  left_back_data=left_back_data.(0)
  raw_data=read_ascii(raw_data_path,data_start=1)
  raw_data=raw_data.(0)

  ;build output array
  n_frame=size(raw_data(0,*),/n_element)
  output_array=fltarr(9,n_frame);index F_X1(head) F_X2(tail) L_X1(head) L_X2(tail) F_X1(after scaling) F_X2(afterscaling) L_X1(after scaling) L_X2(afterscaling)
  output_array=output_array-999
  output_array([0,1,2,3,4],*)=raw_data([0,2,3,10,11],*)
  output_array(1:2,*)=output_array(1:2,*)+translation ; translate the F_camera

  ;build quadrant array
  quadrant=fltarr(2,n_frame);F_side,L_side
  ;fill the data of which both cameras captured
  overlap_index=where(output_array(1,*)eq output_array(1,*) and output_array(3,*) eq output_array(3,*),count)
  for i=long(0),long(count-1) do begin
    if output_array(1,overlap_index(i)) gt 400 then begin
      if output_array(3,overlap_index(i)) lt 400 then begin
        quadrant(0,overlap_index(i))=-1 & quadrant(1,overlap_index(i))=-1
      endif else begin
        quadrant(0,overlap_index(i))=1 & quadrant(1,overlap_index(i))=-1
      endelse
    endif
    if output_array(1,overlap_index(i)) lt 400 then begin
      if output_array(3,overlap_index(i)) lt 400 then begin
        quadrant(0,overlap_index(i))=-1 & quadrant(1,overlap_index(i))=1
      endif else begin
        quadrant(0,overlap_index(i))=1 & quadrant(1,overlap_index(i))=1
      endelse
    endif
  endfor
  ;fill the blank before first overlaped data
  if overlap_index(0) ne 0 then begin
    if output_array(1,0)eq output_array(1,0) then quadrant(0,0:overlap_index(0)-1)=quadrant(0,0:overlap_index(0)-1)+quadrant(0,overlap_index(0))
    if output_array(3,0)eq output_array(3,0) then quadrant(1,0:overlap_index(0)-1)=quadrant(1,0:overlap_index(0)-1)+quadrant(1,overlap_index(0))
  endif
  ;fill in the quadrant data
  ;if the valus is NaN, quadrant=0.Else,quadrant(j)=quadrant(j-1)
  for j=overlap_index(0)+1,long(n_frame-1) do begin
    if output_array(1,j)eq output_array(1,j) then begin
      if quadrant(0,j-1)ne 0 then begin
        quadrant(0,j)=quadrant(0,j-1)
      endif else begin
        if quadrant(0,j-2)ne 0 then quadrant(0,j)=quadrant(0,j-2)
      endelse 
    endif
    if output_array(3,j)eq output_array(3,j) then begin
      if quadrant(1,j-1)ne 0 then begin 
        quadrant(1,j)=quadrant(1,j-1)
      endif else begin
        if quadrant(1,j-2)ne 0 then quadrant(1,j)=quadrant(1,j-2)
      endelse 
    endif
  endfor

  ;find out the center of the chain
  for k=long(0),long(n_frame-1) do begin
    if k mod 10000 eq 0 then print,k
    ;F=1
    if quadrant(0,k)eq 1 and where(output_array(1,k)eq forward_front_data(0,*))ne-1 and where(output_array(2,k)eq forward_front_data(0,*))ne-1 then begin
      F_X1=forward_front_data(2,where(output_array(1,k)eq forward_front_data(0,*)))
      F_X2=forward_front_data(2,where(output_array(2,k)eq forward_front_data(0,*)))
      output_array(5:6,k)=!pi*1.5+GPT10_angle_correction_ver2(F_X1,F_X2,1) ;ruler[-pi/2:pi/2] -> angle[pi:2*pi]

    endif
    ;F=-1
    if quadrant(0,k)eq -1 and where(output_array(1,k)eq forward_back_data(0,*))ne-1 and where(output_array(2,k)eq forward_back_data(0,*))ne-1 then begin
      F_X1=forward_back_data(2,where(output_array(1,k)eq forward_back_data(0,*)))
      F_X2=forward_back_data(2,where(output_array(2,k)eq forward_back_data(0,*)))
      output_array(5:6,k)=!pi/2-GPT10_angle_correction_ver2(F_X1,F_X2,-1)  ;ruler[-pi/2:pi/2] -> angle[pi:0]
      
    endif
    ;L=1
    if quadrant(1,k)eq 1 and where(output_array(3,k)eq Left_front_data(0,*))ne -1 and where(output_array(4,k)eq Left_front_data(0,*))ne-1 then begin
      L_X1=Left_front_data(2,where(output_array(3,k)eq Left_front_data(0,*)))
      L_X2=Left_front_data(2,where(output_array(4,k)eq Left_front_data(0,*)))
      output_array(7:8,k)=!pi+GPT10_angle_correction_ver2(L_X1,L_X2,1)  ;ruler[-pi/2:pi/2] -> angle[pi/2:3*pi/2]
    endif
    ;L=-1
    if quadrant(1,k)eq -1 and where(output_array(3,k)eq Left_back_data(0,*))ne -1 and where(output_array(4,k)eq Left_back_data(0,*))ne -1 then begin
      L_X1=Left_back_data(2,where(output_array(3,k)eq Left_back_data(0,*)))
      L_X2=Left_back_data(2,where(output_array(4,k)eq Left_back_data(0,*)))
      output_array(7:8,k)=(!pi*2-GPT10_angle_correction_ver2(L_X1,L_X2,-1))mod(!pi*2)  ;ruler[-pi/2:pi/2] -> angle[pi/2:3*pi/2]
    endif

  endfor
  
  return,output_array
end

raw_data_file='D:\working space\GPT10\GPT10_20150207_N=5~8_combined_tracking_G=1.55_2.00_25Hz_Sun20150313\*N=5*.txt'
output_path='D:\working space\GPT10\GPT10_chain_length_variation_Jia20160301\'
file_path=file_search(raw_data_file)
n_file=size(file_path,/n_element)
translation=3; F_camera's translation in pixel

print,'There are total '+strtrim(string(n_file),2)+' files to do.'
for i=0,n_file-1 do begin
  print,'This is the '+strtrim(string(i+1),2)+'th time.'
  output_array=GPT10_chain_length_reader(file_path(i),translation)
  
  raw_name=strsplit(file_path(i),'\',count=n_string,/extract)
  name=strmid(raw_name(n_string-1),0,strlen(raw_name(n_string-1))-4)
  file_name=name+'_chain_length_Jia_20160803.txt'
  openw,_data,output_path+file_name,/get_lun
  printf,_data,'        index        F_X1(head)        F_X2(tail)        L_X1(head)        L_X2(tail)        F_X1(after scaling)        F_X2(afterscaling)        L_X1(after scaling)        L_X2(afterscaling)'
  printf,_data,output_array,FORMAT='(9(4x,F12.5))' 
  close,_data    
endfor
print,'done'    
end